/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/include/agvNavigate/guide_help.h
 * @Description  : GuideHelper类的接口定义，用于处理轨迹跟踪和定位反馈。
 *                 提供了设置轨迹、速度、收集信息、生成控制量以及处理定位反馈等功能。 
 * @Author       : xiujun.yang 
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 16:20:24
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#pragma once
#include "trajectory.h"
#include "motion_parameters.h"
#include "follow_path.h"

enum class LocationFeedback {
    LOC_FEEDBACK_OK,
    LOC_FEEDBACK_WAITING,
    LOC_DEAD_RECKONING,
    LOC_FEEDBACK_FAIL
};

//////////////////////////////////////////////////////////////////////////////
//  The interface of class "GuideHelper".
class GuideHelper
{
 public:
    // Constructor
    GuideHelper();
    ~GuideHelper() = default;
    
    // 创建
    void Create(bool bCheckMark);

    // 设置当前的轨迹
    void SetTraj(Trajectory* trajectory, float area);

    // 设置当前规划的线速度
    void SetLinearVel(float linear_velocity) { linear_velocity_ = linear_velocity; }

    // 设置当前规划的角速度
    void SetAngularVel(float angular_velocity) { angular_velocity_ = angular_velocity; }

    // 当轨迹开始时，调用一次
    void OnStart(float dead_reckoning_limit);

    // 收集一些必要的信息
    void CollectInfo(Posture& posture, bool only_collect_info = false);

    // 遍历查找对应速度等级下的纠偏系数
    void FindLevelParam(float& a1, float& a2);

    // 产生纠偏量，实现自动轨迹跟踪
    ModifyCtrl GenAngularVel(const Posture& posture, bool& feedback_ok, bool ignore_y_error = false);

    // 获取评估当前在轨迹上的相对进度(单位:m)
    bool GetMeasuredRelProgress(float& relative_progress);

    // 过滤实时的反馈定位数据有效性
    void FilterFeedBack(bool hold_current_feedback, bool& current_feedback);

    // 试图定位数据超时或不可靠时，盲走一段距离，等待导航数据恢复
    void TryToDeadReckonRun(bool& current_feedback);

    // 返回当前投影的定位偏差
    ProjectData GetProjectData() const { return project_data_; }

    // 返回定位置信度
    float GetQuality() const { return quality_; }

    // 返回当前反馈定位状态
    LocationFeedback GetLocFeedBack() const { return location_feedback_; }
    
    //根据当前速度获取当前的角角速度值
    float GetAngleVelStep(const float& velocity) const;

 public:
    static ModifyCtrl modify_ctrl_; // 纠偏量
 private:
    // 初始化变量数值
    void InitValue();

    Trajectory *trajectory_;         // Pointer to the trajectory object
    float linear_velocity_;          // The current linear velocity
    float angular_velocity_;         // The current angular velocity
    float area_remain_;              // The current area left
    // ModifyCtrl m_modifyCtrl;     // 纠偏量
    float measured_progress_;        // The measured progress value
    USHORT wait_cycle_;              // Count of cycles
    float dead_reckoning_distance_;  // Dead reckoning distance
    float dead_reckoning_limit_;     // Dead-reckoning limit distance
    short normal_count_;             // Count "feedback OK" segments
    short failed_count_;             // Count of "no beedback" cycles
    ProjectData project_data_;       // The project data
    float quality_;                  // Quality of laser measurement
    LocationFeedback location_feedback_;  // Laser feedback type

    // 如果使用地标信号停车，则启用此标志(暂时不启用)
    bool check_mark_;                // Is it necessary to check mark?
    bool mark_finished_;             // Flag-a mark has been checked
    bool signal_triggered_;          // Whether the mark is detected bSignalTriggerL
};

